package adfc2.core;

import java.util.BitSet;
import java.util.PriorityQueue;

import static adfc2.core.World.*;

/**
 * A*, ничего оригинального. Не оптимизирован.
 */
public class Pathfinder {

    static final class Path implements Comparable<Path> {
        final int g; // квадрат пройдённого расстояния
        final int h; // эвристика
        final int steps;
        final int lastxy;
        final Path prev;

        Path(int dg, int h, int lastxy, Path prev) {
            this.g = (prev == null) ? dg : (prev.g + dg);
            this.h = h;
            this.lastxy = lastxy;
            this.prev = prev;
            this.steps = (prev == null) ? 1 : (prev.steps + 1);
        }

        public int compareTo(Path o) {
            return (g + h) - (o.g + o.h);
        }
    }

    /**
     * Возвращает массив xy-координат пути, включая начальную и конечную точки.
     * <p/>
     * Или null, если путь не найден.
     *
     * @param near Если true, то если целевая ячейка непроходима, алгоритм выйдет на пути к ячейке рядом с целевой
     */
    public static int[] findPath(int startXY, int targetXY, boolean near) {
        if (!passable(world().blocks[startXY])) return null;
        if (!near && !passable(world().blocks[targetXY])) return null;
        PriorityQueue<Path> paths = new PriorityQueue<Path>();
        BitSet visited = new BitSet(targetXY);
        paths.add(new Path(0, dist2(startXY, targetXY), startXY, null));
        visited.set(startXY);
        while (true) {
            if (paths.isEmpty()) return null;
            // shortest
            Path path = paths.poll();
            // expand
            int x = xy2x(path.lastxy);
            int y = xy2y(path.lastxy);
            for (int dx = -1; dx <= 1; dx++) {
                for (int dy = -1; dy <= 1; dy++) {
                    if (dy == 0 && dx == 0) continue;
                    int stepx = x + dx;
                    int stepy = y + dy;
                    if (stepx < 0 || stepx >= WORLD_WIDTH || stepy < 0 || stepy >= WORLD_WIDTH) continue;
                    int stepxy = mkxy(x + dx, y + dy);
                    if (visited.get(stepxy)) continue;
                    visited.set(stepxy);
                    if (stepxy == targetXY) {
                        return makePath(path, stepxy);
                    } else if (passable(world().blocks[stepxy])) {
                        paths.add(new Path(dx * dx + dy * dy, dist2(stepxy, targetXY), stepxy, path));
                    }
                }
            }
        }
    }

    private static int[] makePath(Path path, int stepxy) {
        int[] pathxys = new int[path.steps + 1];
        pathxys[path.steps] = stepxy;
        Path ipath = path;
        int i = path.steps - 1;
        while (ipath != null) {
            pathxys[i] = ipath.lastxy;
            i--;
            ipath = ipath.prev;
        }
        return pathxys;
    }
}
